import lejos.nxt.*;
import lejos.nxt.comm.RConsole;

public class test {

	/**
	 * @param args
	 */
	public static void main(String[] args) {
			RConsole.openUSB(0);

            try{
                Thread.sleep(3000);
            }
            catch(InterruptedException e){

            }

            Controller controller = new Controller();
            Gyroscope gyro = new Gyroscope(SensorPort.S1);
            MotorController motor = new MotorController();
            GyroscopeData gd = new GyroscopeData(0,0);
            LightDistancer light = new LightDistancer(SensorPort.S4);
            
            
//            RConsole.println("MAIN");
            
        	Integer i = new Integer(0);

            while(true) {
            	i++;

                gd = gyro.getData();

                controller.setInput(gd.getAngularVelocity(), gd.getAngle());
//                RConsole.println(i.toString());
//                RConsole.println("Angle: " + Integer.valueOf(gd.getAngle()));
//                RConsole.println("Angular velocity: " + Integer.valueOf(gd.getAngularVelocity()));
//                RConsole.println(Integer.valueOf(controller.getOutput()).toString());
//                RConsole.println(Integer.valueOf(gd.getAngle()).toString());
                int output = controller.getOutput();
                RConsole.println(gd.getAngle()+"; "+gd.getAngularVelocity()+"; "+output+";");
                motor.setMotorSpeed(output);
                if (light.getData() == 0) {
                	gyro.resetAngle();
//                	RConsole.println("\nreset KURWA!\n");
                }

            }   
	}

}
